
#include "IVoronoiSimplexSolver.h"

#define VERTA 0
#define VERTB 1
#define VERTC 2
#define VERTD 3

#define CATCH_DEGENERATE_TETRAHEDRON 1
void IVoronoiSimplexSolver::removeVertex(int index)
{
	m_numVertices--;
	m_simplexVectorW[index] = m_simplexVectorW[m_numVertices];
	m_simplexPointsP[index] = m_simplexPointsP[m_numVertices];
	m_simplexPointsQ[index] = m_simplexPointsQ[m_numVertices];
}

void IVoronoiSimplexSolver::reduceVertices()
{
	if ((numVertices() >= 4) && (!m_cachedBC.usedVertex[3]))
		removeVertex(3);

	if ((numVertices() >= 3) && (!m_cachedBC.usedVertex[2]))
		removeVertex(2);

	if ((numVertices() >= 2) && (!m_cachedBC.usedVertex[1]))
		removeVertex(1);

	if ((numVertices() >= 1) && (!m_cachedBC.usedVertex[0]))
		removeVertex(0);
}

IVoronoiSimplexSolver::IVoronoiSimplexSolver()
{
#ifdef REAL_IS_DOUBLE
	m_equalVertexThreshold = 1e-12f;
#else
	m_equalVertexThreshold = 0.0001f;
#endif
}

void IVoronoiSimplexSolver::reset()
{
	m_cachedValidClosest = false;
	m_numVertices = 0;
	m_needsUpdate = true;
	m_lastW = BVector(BReal(REAL_LARGE), BReal(REAL_LARGE), BReal(REAL_LARGE));
	m_cachedBC.reset();
}

//add a vertex
void IVoronoiSimplexSolver::addVertex(const BVector& w, const BVector& p, const BVector& q)
{
	m_lastW = w;
	m_needsUpdate = true;

	m_simplexVectorW[m_numVertices] = w;
	m_simplexPointsP[m_numVertices] = p;
	m_simplexPointsQ[m_numVertices] = q;

	m_numVertices++;
}

bool IVoronoiSimplexSolver::updateClosestVectorAndPoints()
{
	if (m_needsUpdate)
	{
		m_cachedBC.reset();

		m_needsUpdate = false;

		switch (numVertices())
		{
			case 0:
				m_cachedValidClosest = false;
				break;
			case 1:
			{
				m_cachedP1 = m_simplexPointsP[0];
				m_cachedP2 = m_simplexPointsQ[0];
				m_cachedV = m_cachedP1 - m_cachedP2;  //== m_simplexVectorW[0]
				m_cachedBC.reset();
				m_cachedBC.setBarycentricCoordinates(BReal(1.), BReal(0.), BReal(0.), BReal(0.));
				m_cachedValidClosest = m_cachedBC.isValid();
				break;
			};
			case 2:
			{
				//closest point origin from line segment
				const BVector& from = m_simplexVectorW[0];
				const BVector& to = m_simplexVectorW[1];
				BVector nearest;

				BVector p(BReal(0.), BReal(0.), BReal(0.));
				BVector diff = p - from;
				BVector v = to - from;
				BReal t = v.dot(diff);

				if (t > 0)
				{
					BReal dotVV = v.dot(v);
					if (t < dotVV)
					{
						t /= dotVV;
						diff -= t * v;
						m_cachedBC.usedVertex[0] = true;
						m_cachedBC.usedVertex[1] = true;
					}
					else
					{
						t = 1;
						diff -= v;
						//reduce to 1 point
						m_cachedBC.usedVertex[1] = true;
					}
				}
				else
				{
					t = 0;
					//reduce to 1 point
					m_cachedBC.usedVertex[0] = true;
				}
				m_cachedBC.setBarycentricCoordinates(1 - t, t);
				nearest = from + t * v;

				m_cachedP1 = m_simplexPointsP[0] + t * (m_simplexPointsP[1] - m_simplexPointsP[0]);
				m_cachedP2 = m_simplexPointsQ[0] + t * (m_simplexPointsQ[1] - m_simplexPointsQ[0]);
				m_cachedV = m_cachedP1 - m_cachedP2;

				reduceVertices();

				m_cachedValidClosest = m_cachedBC.isValid();
				break;
			}
			case 3:
			{
				//closest point origin from triangle
				BVector p(BReal(0.), BReal(0.), BReal(0.));

				const BVector& a = m_simplexVectorW[0];
				const BVector& b = m_simplexVectorW[1];
				const BVector& c = m_simplexVectorW[2];

				closestPtPointTriangle(p, a, b, c, m_cachedBC);
				m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] +
							 m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] +
							 m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2];

				m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoords[0] +
							 m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] +
							 m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2];

				m_cachedV = m_cachedP1 - m_cachedP2;

				reduceVertices();
				m_cachedValidClosest = m_cachedBC.isValid();

				break;
			}
			case 4:
			{
				BVector p(BReal(0.), BReal(0.), BReal(0.));

				const BVector& a = m_simplexVectorW[0];
				const BVector& b = m_simplexVectorW[1];
				const BVector& c = m_simplexVectorW[2];
				const BVector& d = m_simplexVectorW[3];

				bool hasSeparation = closestPtPointTetrahedron(p, a, b, c, d, m_cachedBC);

				if (hasSeparation)
				{
					m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] +
								 m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] +
								 m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2] +
								 m_simplexPointsP[3] * m_cachedBC.m_barycentricCoords[3];

					m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoords[0] +
								 m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] +
								 m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2] +
								 m_simplexPointsQ[3] * m_cachedBC.m_barycentricCoords[3];

					m_cachedV = m_cachedP1 - m_cachedP2;
					reduceVertices();
				}
				else
				{
					//					printf("sub distance got penetration\n");

					if (m_cachedBC.m_degenerate)
					{
						m_cachedValidClosest = false;
					}
					else
					{
						m_cachedValidClosest = true;
						//degenerate case == false, penetration = true + zero
						m_cachedV.set(BReal(0.), BReal(0.), BReal(0.));
					}
					break;
				}

				m_cachedValidClosest = m_cachedBC.isValid();

				//closest point origin from tetrahedron
				break;
			}
			default:
			{
				m_cachedValidClosest = false;
			}
		};
	}

	return m_cachedValidClosest;
}

//return/calculate the closest vertex
bool IVoronoiSimplexSolver::closest(BVector& v)
{
	bool succes = updateClosestVectorAndPoints();
	v = m_cachedV;
	return succes;
}

BReal IVoronoiSimplexSolver::maxVertex()
{
	int i, numverts = numVertices();
	BReal maxV = BReal(0.);
	for (i = 0; i < numverts; i++)
	{
		BReal curLen2 = m_simplexVectorW[i].length2();
		if (maxV < curLen2)
			maxV = curLen2;
	}
	return maxV;
}

//return the current simplex
int IVoronoiSimplexSolver::getSimplex(BVector* pBuf, BVector* qBuf, BVector* yBuf) const
{
	int i;
	for (i = 0; i < numVertices(); i++)
	{
		yBuf[i] = m_simplexVectorW[i];
		pBuf[i] = m_simplexPointsP[i];
		qBuf[i] = m_simplexPointsQ[i];
	}
	return numVertices();
}

bool IVoronoiSimplexSolver::inSimplex(const BVector& w)
{
	bool found = false;
	int i, numverts = numVertices();
	//BReal maxV = BReal(0.);

	//w is in the current (reduced) simplex
	for (i = 0; i < numverts; i++)
	{
#ifdef BT_USE_EQUAL_VERTEX_THRESHOLD
		if ((m_simplexVectorW[i] - w).length2() <= m_equalVertexThreshold)
#else
		if (m_simplexVectorW[i] == w)
#endif
		{
			found = true;
			break;
		}
	}

	//check in case lastW is already removed
	if (w == m_lastW)
		return true;

	return found;
}

void IVoronoiSimplexSolver::backup_closest(BVector& v)
{
	v = m_cachedV;
}

bool IVoronoiSimplexSolver::emptySimplex() const
{
	return (numVertices() == 0);
}

void IVoronoiSimplexSolver::compute_points(BVector& p1, BVector& p2)
{
	updateClosestVectorAndPoints();
	p1 = m_cachedP1;
	p2 = m_cachedP2;
}

bool IVoronoiSimplexSolver::closestPtPointTriangle(const BVector& p, const BVector& a, const BVector& b, const BVector& c, SimplexClosestResult& result)
{
	result.usedVertex[0] = false;
	result.usedVertex[1] = false;
	result.usedVertex[2] = false;
	result.usedVertex[3] = false;

	// Check if P in vertex region outside A
	BVector ab = b - a;
	BVector ac = c - a;
	BVector ap = p - a;
	BReal d1 = ab.dot(ap);
	BReal d2 = ac.dot(ap);
	if (d1 <= BReal(0.0) && d2 <= BReal(0.0))
	{
		result.m_closestPointOnSimplex = a;
		result.usedVertex[0] = true;
		result.setBarycentricCoordinates(1, 0, 0);
		return true;  // a; // barycentric coordinates (1,0,0)
	}

	// Check if P in vertex region outside B
	BVector bp = p - b;
	BReal d3 = ab.dot(bp);
	BReal d4 = ac.dot(bp);
	if (d3 >= BReal(0.0) && d4 <= d3)
	{
		result.m_closestPointOnSimplex = b;
		result.usedVertex[1] = true;
		result.setBarycentricCoordinates(0, 1, 0);

		return true;  // b; // barycentric coordinates (0,1,0)
	}
	// Check if P in edge region of AB, if so return projection of P onto AB
	BReal vc = d1 * d4 - d3 * d2;
	if (vc <= BReal(0.0) && d1 >= BReal(0.0) && d3 <= BReal(0.0))
	{
		BReal v = d1 / (d1 - d3);
		result.m_closestPointOnSimplex = a + v * ab;
		result.usedVertex[0] = true;
		result.usedVertex[1] = true;
		result.setBarycentricCoordinates(1 - v, v, 0);
		return true;
		//return a + v * ab; // barycentric coordinates (1-v,v,0)
	}

	// Check if P in vertex region outside C
	BVector cp = p - c;
	BReal d5 = ab.dot(cp);
	BReal d6 = ac.dot(cp);
	if (d6 >= BReal(0.0) && d5 <= d6)
	{
		result.m_closestPointOnSimplex = c;
		result.usedVertex[2] = true;
		result.setBarycentricCoordinates(0, 0, 1);
		return true;  //c; // barycentric coordinates (0,0,1)
	}

	// Check if P in edge region of AC, if so return projection of P onto AC
	BReal vb = d5 * d2 - d1 * d6;
	if (vb <= BReal(0.0) && d2 >= BReal(0.0) && d6 <= BReal(0.0))
	{
		BReal w = d2 / (d2 - d6);
		result.m_closestPointOnSimplex = a + w * ac;
		result.usedVertex[0] = true;
		result.usedVertex[2] = true;
		result.setBarycentricCoordinates(1 - w, 0, w);
		return true;
		//return a + w * ac; // barycentric coordinates (1-w,0,w)
	}

	// Check if P in edge region of BC, if so return projection of P onto BC
	BReal va = d3 * d6 - d5 * d4;
	if (va <= BReal(0.0) && (d4 - d3) >= BReal(0.0) && (d5 - d6) >= BReal(0.0))
	{
		BReal w = (d4 - d3) / ((d4 - d3) + (d5 - d6));

		result.m_closestPointOnSimplex = b + w * (c - b);
		result.usedVertex[1] = true;
		result.usedVertex[2] = true;
		result.setBarycentricCoordinates(0, 1 - w, w);
		return true;
		// return b + w * (c - b); // barycentric coordinates (0,1-w,w)
	}

	// P inside face region. Compute Q through its barycentric coordinates (u,v,w)
	BReal denom = BReal(1.0) / (va + vb + vc);
	BReal v = vb * denom;
	BReal w = vc * denom;

	result.m_closestPointOnSimplex = a + ab * v + ac * w;
	result.usedVertex[0] = true;
	result.usedVertex[1] = true;
	result.usedVertex[2] = true;
	result.setBarycentricCoordinates(1 - v - w, v, w);

	return true;
	//	return a + ab * v + ac * w; // = u*a + v*b + w*c, u = va * denom = BReal(1.0) - v - w
}

/// Test if point p and d lie on opposite sides of plane through abc
int IVoronoiSimplexSolver::pointOutsideOfPlane(const BVector& p, const BVector& a, const BVector& b, const BVector& c, const BVector& d)
{
	BVector normal = (b - a).cross(c - a);

	BReal signp = (p - a).dot(normal);  // [AP AB AC]
	BReal signd = (d - a).dot(normal);  // [AD AB AC]

#ifdef CATCH_DEGENERATE_TETRAHEDRON
#ifdef BT_USE_DOUBLE_PRECISION
	if (signd * signd < (BReal(1e-8) * BReal(1e-8)))
	{
		return -1;
	}
#else
	if (signd * signd < (BReal(1e-4) * BReal(1e-4)))
	{
		//		printf("affine dependent/degenerate\n");//
		return -1;
	}
#endif

#endif
	// Points on opposite sides if expression signs are opposite
	return signp * signd < BReal(0.);
}

bool IVoronoiSimplexSolver::closestPtPointTetrahedron(const BVector& p, const BVector& a, const BVector& b, const BVector& c, const BVector& d, SimplexClosestResult& finalResult)
{
	SimplexClosestResult tempResult;

	// Start out assuming point inside all halfspaces, so closest to itself
	finalResult.m_closestPointOnSimplex = p;
	finalResult.usedVertex[0] = true;
	finalResult.usedVertex[1] = true;
	finalResult.usedVertex[2] = true;
	finalResult.usedVertex[3] = true;

	int pointOutsideABC = pointOutsideOfPlane(p, a, b, c, d);
	int pointOutsideACD = pointOutsideOfPlane(p, a, c, d, b);
	int pointOutsideADB = pointOutsideOfPlane(p, a, d, b, c);
	int pointOutsideBDC = pointOutsideOfPlane(p, b, d, c, a);

	if (pointOutsideABC < 0 || pointOutsideACD < 0 || pointOutsideADB < 0 || pointOutsideBDC < 0)
	{
		finalResult.m_degenerate = true;
		return false;
	}

	if (!pointOutsideABC && !pointOutsideACD && !pointOutsideADB && !pointOutsideBDC)
	{
		return false;
	}

	BReal bestSqDist = REAL_MAX;
	// If point outside face abc then compute closest point on abc
	if (pointOutsideABC)
	{
		closestPtPointTriangle(p, a, b, c, tempResult);
		BVector q = tempResult.m_closestPointOnSimplex;

		BReal sqDist = (q - p).dot(q - p);
		// Update best closest point if (squared) distance is less than current best
		if (sqDist < bestSqDist)
		{
			bestSqDist = sqDist;
			finalResult.m_closestPointOnSimplex = q;
			//convert result bitmask!
			finalResult.usedVertex[0] = tempResult.usedVertex[0];
			finalResult.usedVertex[1] = tempResult.usedVertex[1];
			finalResult.usedVertex[2] = tempResult.usedVertex[2];
			finalResult.setBarycentricCoordinates(
				tempResult.m_barycentricCoords[VERTA],
				tempResult.m_barycentricCoords[VERTB],
				tempResult.m_barycentricCoords[VERTC],
				0);
		}
	}

	// Repeat test for face acd
	if (pointOutsideACD)
	{
		closestPtPointTriangle(p, a, c, d, tempResult);
		BVector q = tempResult.m_closestPointOnSimplex;
		//convert result bitmask!

		BReal sqDist = (q - p).dot(q - p);
		if (sqDist < bestSqDist)
		{
			bestSqDist = sqDist;
			finalResult.m_closestPointOnSimplex = q;
			finalResult.usedVertex[0] = tempResult.usedVertex[0];
			finalResult.usedVertex[2] = tempResult.usedVertex[1];
			finalResult.usedVertex[3] = tempResult.usedVertex[2];
			finalResult.setBarycentricCoordinates(
				tempResult.m_barycentricCoords[VERTA],
				0,
				tempResult.m_barycentricCoords[VERTB],
				tempResult.m_barycentricCoords[VERTC]);
		}
	}
	// Repeat test for face adb

	if (pointOutsideADB)
	{
		closestPtPointTriangle(p, a, d, b, tempResult);
		BVector q = tempResult.m_closestPointOnSimplex;
		//convert result bitmask!

		BReal sqDist = (q - p).dot(q - p);
		if (sqDist < bestSqDist)
		{
			bestSqDist = sqDist;
			finalResult.m_closestPointOnSimplex = q;
			finalResult.usedVertex[0] = tempResult.usedVertex[0];
			finalResult.usedVertex[1] = tempResult.usedVertex[2];
			finalResult.usedVertex[3] = tempResult.usedVertex[1];
			finalResult.setBarycentricCoordinates(
				tempResult.m_barycentricCoords[VERTA],
				tempResult.m_barycentricCoords[VERTC],
				0,
				tempResult.m_barycentricCoords[VERTB]);
		}
	}
	// Repeat test for face bdc

	if (pointOutsideBDC)
	{
		closestPtPointTriangle(p, b, d, c, tempResult);
		BVector q = tempResult.m_closestPointOnSimplex;
		//convert result bitmask!
		BReal sqDist = (q - p).dot(q - p);
		if (sqDist < bestSqDist)
		{
			bestSqDist = sqDist;
			finalResult.m_closestPointOnSimplex = q;
			finalResult.usedVertex[1] = tempResult.usedVertex[0];
			finalResult.usedVertex[2] = tempResult.usedVertex[2];
			finalResult.usedVertex[3] = tempResult.usedVertex[1];
			finalResult.setBarycentricCoordinates(
				0,
				tempResult.m_barycentricCoords[VERTA],
				tempResult.m_barycentricCoords[VERTC],
				tempResult.m_barycentricCoords[VERTB]);
		}
	}

	//help! we ended up full !

	if (finalResult.usedVertex[0] &&
		finalResult.usedVertex[1] &&
		finalResult.usedVertex[2] &&
		finalResult.usedVertex[3])
	{
		return true;
	}

	return true;
}
